# lidar default :
# ip addr:192.168.1.200
# UDP equipment socket:2368
# UDP data socket:2369 
# 通过ethernet连接，直接获取包数据并处理
# 通过socket接收数据
import socket
import math

vangle = [
    -15,
    1,
    -13,
    3,
    -11,
    5,
    -9,
    7,
    -7,
    9,
    -5,
    11,
    -3,
    13,
    -1,
    15,
]

# 设置为全局变量
sock = None


def init_lidar():
    global sock
    if sock is None:
        sock = lidar_socket()


def get_lidar_data():
    global sock
    return lidar_data(sock)


def shutdown_lidar():
    global sock
    if sock:
        lidar_close(sock)
        sock = None


def lidar_socket(ip='192.168.1.200', port=2368, size=1024 * 1024):
    # 创建一个UDP socket
    sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    # 绑定到指定的IP地址和端口
    sock.bind((ip, port))
    # 设置socket为非阻塞模式
    sock.setblocking(0)
    # 设置接收数据的缓冲区大小
    sock.setsockopt(socket.SOL_SOCKET, socket.SO_RCVBUF, size)
    # 设置接收数据的超时时间
    sock.settimeout(1)

    return sock


# 数据包格式:
# 1248B = header(42B) + data(1206B)
def lidar_data(sock):
    # data是接收到的数据，addr是发送数据的地址
    data, addr = sock.recvfrom(1024 * 1024)
    # 
    print("Received data from:", addr)

    # 分开header和data
    header = data[:42]
    data = data[42:]

    return header, data


def lidar_close(sock):
    # 关闭socket
    sock.close()
    print("Socket closed.")


# 将小端模式的两字节数据结合(无符号)
def combine_bytes(data):
    return (data[0] | (data[1] << 8))


# data格式:
# 12block + 4B timestamp +2B Factory
# 每个block 100B = 2B flag + 2B azimuth + (2B distance + 1B intensity) * 2 * 16
# 即一个点云数据包含12个数据块,每个数据块包含2组按打包顺序测量的16个通道点云数据,
# 每个数据块只返回一个方位角, 每个方位角输出2组数据
def xyz_process(data):
    print("Processing data...")
    # 解析数据
    block = []
    azimuth = []
    for i in range(12):
        block.append(data[i * 100:(i + 1) * 100])
        azimuth.append(combine_bytes(block[i][2:4]))
    timestamp = data[1200:1204]  # 微秒(未处理数据)
    factory = data[1204:1206]
    # 解析每个block
    for i in range(12):
        # 解析flag
        flag = block[i][:2]
        # 解析距离\强度\水平角
        distance = []
        intensity = []
        angle = []
        for j in range(2):
            for k in range(16):
                distance.append(2.5 * combine_bytes(block[i][4 + (j * 16 + k) * 3:4 + (j * 16 + k) * 3 + 2]))  # 单位mm
                intensity.append(block[i][4 + (j * 16 + k) * 3 + 2])
                if i != 0:
                    delta = azimuth[i] - azimuth[i - 1]
                else:
                    delta = azimuth[1] - azimuth[0]
                angle.append((azimuth[i] + delta * (j / 2 + k / 16)) * 0.01)  # 角度

    xyz = []
    for i in range(len(distance)):
        x = distance[i] * math.cos(angle[i] * math.pi / 180) * math.cos(vangle[i % 16] * math.pi / 180)
        y = distance[i] * math.sin(angle[i] * math.pi / 180) * math.cos(vangle[i % 16] * math.pi / 180)
        z = distance[i] * math.sin(vangle[i % 16] * math.pi / 180)
        xyz.append([x, y, z])

    return xyz, timestamp, factory, flag


def recv_laser_data():
    header, data = get_lidar_data()
    xyz, _ = xyz_process(data)
    return xyz


if __name__ == '__main__':
    init_lidar()
    while True:
        try:
            header, data = get_lidar_data()
            xyz, timestamp, _ = xyz_process(data)
            print(xyz[:5])
        except KeyboardInterrupt:
            print("\n已手动中断。")
            break
    shutdown_lidar()
